#ifndef _MOYING_COSTMAP_H_
#define _MOYING_COSTMAP_H_

#include <vector>
#include <boost/thread.hpp>
#include <utilities/utilities.h>

namespace moying
{
    namespace navigation
    {
        enum MAPCOSTS
        {
            FREE = 0,
            INFLATED_OBSTACLE = 100,
            OBSTACLE = 255
        };

        class CostMap
        {
            public:
                CostMap();
                virtual ~CostMap();
                
                inline bool isReady()
                {
                    return is_ready_;
                }

                bool loadStaticMapFromFile(const std::string &fname, double res, bool negate, 
                                           double occ_th, double free_th, std::vector<double> &origin);

                bool setupPlanningCostMap(double planning_res);

                void inflateStaticMap(double inflation_radius);

                void setObstacle(int mx, int my);

                bool rawMapToWorld(int mx, int my, double& wx, double& wy) ;
                bool mapToWorld(int mx, int my, double& wx, double& wy) ;
                inline bool mapToRawMap(int mx,int my,int& rmx,int& rmy ){
                    if(mx >= size_x_ || my >= size_y_ || mx < 0 || my < 0)
                        return false;
                    int rate = resolution_/raw_resolution_;
                    rmx = (mx+0.5) * rate;
                    rmy = (my+0.5) * rate;

                    rmx += map_offset_x_;
                    rmy += map_offset_y_;
                    rmx = rmx < 0 ? 0 : rmx;
                    rmx = rmx >= raw_size_x_ ? raw_size_x_-1 : rmx;
                    rmy = rmy < 0 ? 0 : rmy;
                    rmy = rmy >= raw_size_y_ ? raw_size_y_-1 : rmy;
                    return true;
                }

                inline bool rawMapToMap(int rmx,int rmy,int& mx,int& my ){
                    if(rmx >= raw_size_x_ || rmy >= raw_size_y_ || rmx < 0 || rmy < 0)
                        return false;
                    float rate = raw_resolution_/resolution_;
                    rmx -= map_offset_x_;
                    rmy -= map_offset_y_;
                    mx = rmx * rate;
                    my = rmy * rate;

                    mx = mx < 0 ? 0 : mx;
                    mx = mx >= size_x_ ? size_x_-1 : mx;
                    my = my < 0 ? 0 : my;
                    my = my >= size_y_ ? size_y_-1 : my;
                    return true;
                }

                bool worldToRawMap(double wx, double wy, int& mx, int& my) ;
                bool worldToMap(double wx, double wy, int& mx, int& my) ;

                inline bool setCost(int mx,int my,unsigned char cost){
                    if(mx >= size_x_ || my >= size_y_ || mx < 0 || my < 0)
                        return false;
                    costs_[mx][my] = cost;
                    return true;
                }

                inline unsigned char getCost(int mx, int my) const
                {
                    if(mx >= size_x_ || my >= size_y_ || mx < 0 || my < 0)
                        return OBSTACLE;
                    return costs_[mx][my];
                }

                inline unsigned char getRawCost(int rx, int ry) const
                {
                    if(rx >= raw_size_x_ || ry >= raw_size_y_ || rx < 0 || ry < 0)
                        return OBSTACLE;
                    return raw_costs_[rx][ry];
                }

                inline int getRawSizeInCellsX() const
                {
                    return raw_size_x_;
                }

                inline int getSizeInCellsX() const
                {
                    return size_x_;
                }

                inline int getRawSizeInCellsY() const
                {
                    return raw_size_y_;
                }

                inline int getSizeInCellsY() const
                {
                    return size_y_;
                }

                inline int toIndex(int x, int y)
                {
                    return x + size_x_ * y;
                }
                
                inline double getOriginX() const
                {
                    return origin_x_;
                }
                
                inline double getOriginY() const
                {
                    return origin_y_;
                }

                unsigned char getObstacleCost();

                std::vector<std::vector<unsigned char>>* get_cost_map();

                std::vector<std::vector<unsigned char>>* get_raw_cost_map();

                inline double getRawResolution() const
                {
                    return raw_resolution_;
                }

                inline double getResolution() const
                {
                    return resolution_;
                }

                void normalize_map();

                void getRaw1DCosts(std::vector<unsigned char> &_1dcost);
                void get1DCosts(std::vector<unsigned char> &_1dcost);
                void setMapOffset(int x_offset,int y_offset){
                    map_offset_x_ = x_offset; 
                    map_offset_y_ = y_offset;
                }
                inline int map_offset_x(){return map_offset_x_;}
                inline int map_offset_y(){return map_offset_y_;}

            private:
                std::vector<std::vector<unsigned char>> raw_costs_; //原始的代价地图
                std::vector<std::vector<unsigned char>> costs_;     //改变分辨率的路径规划用的代价地图

                bool is_ready_;
                int raw_size_x_;        //地图原始宽度
                int raw_size_y_;        //地图原始高度
                double raw_resolution_; //原始地图分辨率
                double origin_x_;
                double origin_y_;
                double origin_th_;
                
                double resolution_; //路径规划的地图分辨率
                int size_x_;        //路径规划地图宽度
                int size_y_;        //路径规划地图高度

                int map_offset_x_ = 0;  //地图左下角x坐标偏移
                int map_offset_y_ = 0;  //地图左下角y坐标偏移
        };
        typedef std::shared_ptr<CostMap> CostMapPtr;
    } // namespace navigation
} // namespace moying
#endif //_MOYING_COSTMAP_H_